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Abstract — Kalman Filtering problems often have inherent and 
known constraints in the physical dynamics that are not exploited 
despite potentially significant gains (e.g., fixed speed of a motor). 
In this paper, we review existing methods and propose some new 
ideas for filtering in the presence of equality constraints. We then 
show that three methods for incorporating state space equality 
constraints are mathematically equivalent to the more general 
"Projection" method, which allows different weighting matrices 
when projecting the estimate. Still, the different approaches have 
advantages in implementations that may make one better suited 
than another for a given application. 

Index Terms — Kalman Filter, Equality Constrained Optimiza- 
tion 



I. Introduction 

The Kalman Filter is the optimal estimator for dynamical 
systems with white process noise and measurement noise. 
Since the inception of the Kalman Filter in 1960, a vast 
amount of research has gone into different extensions - for 
example, to allow for nonlinear systems [l]-[5], non-Gaussian 
noise distributions [6]-[9], better numerical stability [10]- 
[17], and state space constraints [18]-[45]. Incorporating these 
extensions gives rise to many sub-fields of Kalman Filtering. 
In the case of handling nonlinearities in the underlying system, 
there are a number of proposed models capturing different 
amounts of detail. This paper will focus on the last problem 
of incorporating state space constraints (that is, improving the 
best estimate given by the filtration process by accounting for 
known impossibilities). This is a small sub-field of Kalman 
Filtering, which has become more popular in just the past few 
decades and is growing rapidly. Specifically, we will focus on 
equality constraints. 

We discuss a few distinct approaches to generaUzing an 
equality constrained Kalman Filter. The first approach is to 
augment the measurement space of the Kalman Filter with the 
equality constraints as noise-free measurements (also called 
pseudo-measurements) [19], [20], [46]. The second approach 
is to find the unconstrained estimate from a Kalman Filter 
and project it down to the equality constrained space [18], 
[21]. The third approach is to restrict the optimal Kalman Gain 
so the updated state estimate lies in the constrained space. 
The fourth approach is to fuse the state prediction with the 
measurement in the constrained space. The second method 
ends up being a generalization of the other three methods, i.e., 
the other methods are all special cases of the second method. 
Proofs for this will be given (see also [27], [28], [47]). Yet 
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another approach to this problem would be to reduce the state 
space by the dimension of the constraints (i.e., to introduce 
an explicit coordinate system in the constrained space). This 
leads to a state space that might not have an intuitive meaning 
in terms of the propagation equations. This approach, while 
valid, is not discussed here. 

Analogous to the way a Kalman Filter can be extended 
to solve problems containing nonlinearities, linear equality 
constrained filtering can be extended to problems with non- 
linear constraints by linearizing locally (or another scheme 
motivated by how nonlinear filters handle the nonlinearities). 
The accuracy achieved by methods dealing with nonlinear 
constraints will naturally depend on the structure and curvature 
of the nonlinear function itself. 

II. Kalman Filter 

The Kalman Filter is a formulation of the recursive least 
squares algorithm, which makes only one pass through the 
data such that it can wait for each measurement to come in 
real time and make an estimate at that time given all the 
information from the past. In addition, the Kalman Filter holds 
a minimal amount of information in memory at each time 
for a cheap computational cost in solving the optimization 
problem. A discrete-time Kalman Filter attempts to find the 
best running estimate for a recursive system governed by the 
following modelQ 



Xk = Fk,k-lXk-l + Ukk-1, 



Zk = HkXk + Wfe, 



Uk,k-1 



' A/'(0,( 



ik,k-l) 



Vk--M{0,Rk) 



(1) 



(2) 



Here Xk is an n-vector that represents the true state of 
the underlying systerrH and Fk,k~i is an n x n matrix that 
describes the transition dynamics of the system from Xk-i to 
Xk- The measurement made by the observer is an m-vector 
Zk, and Hk is an m x n matrix that transforms a vector from 
the state space into the appropriate vector in the measurement 
space. The noise terms Uk^k-i (an n-vector) and Vk (an m- 
vector) encompass errors in Fk^k-i and Hk and are normally 
distributed with mean and covariances given by n x n matrix 

'Deahng with noise that is not normally distributed doesn't lend itself well 
to the framework of the Kalman Filter; for an arbitrary distribution, a Particle 
Filter [48] could be used, and for noises that have heavy tail distributions 
such as power laws and Levy laws, the "Kalman-Levy" Filter has been 
proposed [49], [50]. 

-The subscript k means for the fc-th time step, and all vectors in this paper 
are column vectors (unless of course we are taking the transpose of the vector). 
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Qk,k-i and mx m matrix Rk, respectively. At each iteration, 
the Kalman Filter makes a state prediction for Xk, denoted 
i/c|/c-i- We use the notation k\k — 1 since we will only use 
measurements provided until time-step fc — 1 in order to make 
the prediction at time-step k. The state prediction error a;fe|fe_i 
is defined as the difference between the true state and the state 
prediction, as below. 



^k\k-l — — Xk\k-1 



(3) 



The covariance structure for the expected error on the state 
prediction is defined as the expectation of the outer product 
of the state prediction error We call this covariance structure 
the error covariance prediction and denote it Pk\k-i- 



k\k- 



.1 =E 



[Xk\k-l) [Xk\k-l) 



(4) 



The filter will also provide an updated state estimate for Xk, 
given all the measurements provided up to and including time 
step k. We denote these estimates hy Xk\k- We similarly define 
the state estimate error Xk\k below. 



HkPk\k-iH'k + Rk 



(11) 



We can now define our updated state estimate as our pre- 
diction plus some perturbation, which is given by a weighting 
factor times the measurement residual. The weighting factor, 
called the Kalman Gain, will be discussed below. 



ifc|fe = Xk\k-i +KkVk 



(12) 



Naturally, we can also calculate the updated error covariance 
by expanding the outer product in Equation ([61)0 

Pk\k = (I -KkHk) Pk\k-i (I -KkHk)' + KkRkK (13) 

Now we would like to find the Kalman Gain Kk, which 
minimizes the mean square state estimate error, E 5^1^ . 
This is the same as minimizing the trace of the updated error 
covariance matrix aboveH Expanding Equation ( fTSl ), we have 
the following. After some calculufl we find the optimal gain 
that achieves this, written below. 



Xk\k = Xk- Xk\k 



(5) 



The expectation of the outer product of the state estimate 
error represents the covariance structure of the expected er- 
rors on the state estimate, which we call the updated error 
covariance and denote Pk\k- 



Pk\k — E 



[Xk\k, 



{Xk\h 



(6) 



At time-step fc, we can make a prediction for the underlying 
state of the system by allowing the state to transition forward 
using our model for the dynamics and noting that E [w^ = 
0. This serves as our state prediction. 



Xk\k-1 — Fk,k-lXk-l\k-l 



(7) 



If we expand the expectation in Equation (IDl, we have the 
following equation for the error covariance prediction!! 



Pk\k-1 — FkM-lPk-l\k-lFk.k-l + QkM-1 



(8) 



We can transform our state prediction into the measurement 
space, which is a prediction for the measurement we now 
expect to observe. 



Zk\k-1 = HkXk 



fc-1 



(9) 



The difference between the observed measurement and our 
predicted measurement is the measurement residual, which we 
are hoping to minimize in this algorithm. 



^k — Zk — Zk\k-1 



(10) 



We can also calculate the associated covariance for the 
measurement residual, which is the expectation of the outer 
product of the measurement residual with itself, E [i^fei^^]. We 
call this the measurement residual covariance. 

'We use the prime notation on a vector or a matrix to denote its transpose 
throughout this paper. 



Kk = Pk\k-iH'kSk^ (14) 

Substituting Equation ( fT4l i into Equation ( fT3] l gives the 
following simplified form for the updated error covariance. 



Pk\k = {l-KkHk)Pk\k-i 



(15) 



In computation, one should avoid using this form and use 
Equation ( fT3] l. also called the Joseph Form. While the Joseph 
Form requires more computation, it better preserves symmetry 
and reduces numerical loss of positive definiteness for the 
covariance matrix. 

The covariance matrices in the Kalman Filter provide us 
with a measure for uncertainty in our predictions and updated 
state estimate. This is a very important feature for the various 
applications of filtering since we then know how much to 
trust our predictions and estimates. Also, since the method 
is recursive, we need to provide an initial covariance that is 
large enough to contain the initial state estimate to ensure 
comprehensible performance. 

A. Fusion Interpretation 

We can also think of the Kalman Filter as a fusion of 
the state prediction with the measurement at each iteration. 
Since we know the error covariance matrices for the state 
prediction and the measurement, we can take this fusion under 
a weighting and also calculate a covariance matrix for the 
best estimate. Let us begin by re-writing our system in the 
following mannerQ 

"^The I in Equation 115) represents the n X n identity matrix. Tliroughout 
this paper, we use I to denote the same matrix, except in Appendix |C] in 
which I is the appropriately sized identity matrix. 

'Note that v'v = trace [vv'] for any vector v. 

*The trace is minimized when the following matrix derivative is equal to 
zero: 

d trace[Pfc|fc 



1 = -2 (-H'fcPfc|fe_i)' + 2ii'feSfe = 0. Solving this for yields 



Equation (13). 

'The superscript F notation is used to denote the "fusion" filter. 
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■ Af (0, R 



(16) 



Here and are augmented vectors, and H[ is an 
augmented matrix (see Equations ( [TtT i. ( fTsT i, and (fT9]l). The 
first block of zj^ represents the prediction for the current time 
step, and the second block is the measurement. 



Xk\k- 
Zk 



(17) 



The matrix takes our state into the measurement space, 
as before. 



III. Incorporating Equality Constraints 

Equality constraints in this paper are defined as below, in 
which A is a g X n matrix, h a q-vector, and Xk, the state, is 
a n-vector, with q < n0 



Axk 



(24) 



We would like our updated state estimate to satisfy the 
constraint at each iteration, as below. 



Axk], 



(25) 



Similarly, we may also like the state prediction to be con- 
strained, which would allow a better forecast for the svstemF"! 



I 

Hk 



(18) 



Now we define 



as the noise term, in which 



IS 



normally distributed with mean and covariance given by 
matrix matrix _Rf . 



Xk\k-l 
Vk 



(19) 



The block diagonal elements of represent the covariance 



of each block of Vk ■ 



Notice that R^ contains no block off- 



diagonal elements implying no cross-correlations. However, 
using this formulation, cross-correlations could be modelled 
easily. 



Pk\k-1 







Rk 



(20) 



This method of expressing our problem can be thought of 
as a fusion of the state prediction and the new measurement 
at each iteration. The optimal estimate, as defined by the 
weighted least-squares method, for the system in Equation (fTSl l 
is the minimizer of the cost function below. 



Ax 



kik-l 



(26) 



A. Augmenting the Measurement Space 

The first method that we discuss for incorporating equality 
constraints into a Kalman Filter is to "observe" the constraints 
at each iteration as noise-free measurements (or pseudo- 
measurements). To illustrate this, we augment the linear con- 
straints in Equations ( l24l i to the system shown in Equations 
([T]i and (|2]i as measurements with zero variance. Thus, we can 
re-write the svstemF*! 



xt = Fk.k-iXk^i + UkM-i, 



7^ - B^T^ 
Zk — -"fe ^k 



Uk,k-i ^ Af iO,Qk,k-i) 
(27) 



Vk-^M{0,Rt) 



(28) 



The next three equations show the construction of the 
augmentation in the measurement space. 



zi 



Zk 

b 

Hk 
A 



(29) 
(30) 



J(xfc) = (zf-ijfx,.)'(i?0 '{z^~H[xk) (21) 

The minimizer, which is found by standard calculus, is the 
least squares solution given below. 



= ( (Hi)' (R^)-' H[)" «)' z[ 



The covariance for this solution is the following. 

pL-({Hn'm"H^k 



(22) 



(23) 



Some manipulation shows that this result is the same as that 
of the Kalman Filter0 



°For complete details on this derivation and extensions to nonlinear filtering, 
see [47]. 



Rk 



Rk 




(31) 



The augmented state now forces Ax^ to be equal to b 
exactly (i.e., with no noise term) at every iteration!^ Let us 
now expand the equations for the Kalman Filter prediction and 
update to gain a stronger understanding of how the filter has 
changed. 

'A and b can be different for different k. We don't subscript each A and b 
to avoid confusion. We assume these constraints are well defined throughout 
this paper - i.e., no constraints conflict with one another to cause a null 
solution and no constraints are repeated. More specifically, we assume A has 
full row rank. Note that under these conditions if A was a square matrix, the 
constraints would completely determine the state. 

'"We do not discuss this point further here. For more on this, please 
see [47]. 

' ' The superscript A notation is used to denote the "augmented" constrained 
filter and beai's no relation to the A in Equation j24t . Also, note that the 
dimension of the state space hasn't changed (e.g., is the same size as 

^^x{^ is still constructed in the same fashion as xu. 
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The state prediction from Equation Q becomes the follow- Here, we have used the following two terms to shorten the 
ing. expression above. 



(32) 



The error covariance prediction from Equation (|8]i becomes 
the following. 



pA 

fe|fc-l 



Fk.k-iPti\k-iFU-i + Qk,k-i (33) 



The measurement prediction from Equation (|9]l can then be 
written in the following form. 



-1 — k-1 



(34a) 
(34b) 



Similarly, we can express the measurement residual from 
Equation ( fTOb in the following manner. 



I'l. = Zi. — z 



k\k-l 



Zk HkX^^^_^ 



(35a) 
(35b) 



We expand the measurement residual covariance from Equa- 
tion ( fTTT i below. 



A 



Pklk-AK A'] 



Rk 




HkP^\k-iH'k + Rk H,P^\k-iA' 



AP^\k-iH', 



The Kalman Gain can now be written as below. 



(36a) 
(36b) 

(36c) 



(37) 



In order to further expand this term, we denote (5"^) ^ in 
the following block matrix form. 



{^k)a i^k)b 



(38) 



We then expand the Kalman Gain in terms of the block 
structure of Equation 



K^ = Pk%^i[H', A'] 



i'^k)a^ i^k)b^ 
{^k)c i^k)d 



pA TTl pA A/ 



(39a) 

(39b) 
(39c) 



(Ki)^ = (Si);' + P,%_,A' {SiyJ (40a) 

{K^)b = Pk\k-iH', (Si)-' + P^^,_,A' (Si)-/ (40b) 

Furthermore, the updated state estimate from Equation ( fT2] i 
takes the following form. 



^k\k 



(41) 



And the updated error covariance from Equation (fTSl ) 
changes in the following way. 



Pklk^i^-K^H^)P± 



(42) 



Methods using augmentation in Kalman Filters have ap- 
peared for different applications in the past (e.g., Fixed- 
Point Smoothing [51], Bias Detection [52]). In order to gain 
a stronger understanding of the effects of augmentation in 
Kalman Filters, it can be helpful to read and understand these 
methods, as well - though they are not relevant to equality 
constrained Kalman Filtering. 

1 ) Improvement gained over an Unconstrained Filter: For 
a given iteration, we are interested in the improvement gained 
by using this method over a method that does not incorporate 
equality constraints. In order to do so, we would like to find 
the constrained estimated ij^^, in terms of the unconstrained 
estimate Xk\k (and similarly the constrained error covariance 
matrix P^f^j, in terms of the unconstrained error covariance 
matrix Pk\k)- Suppose we start with the same previous estimate 
and error covariance matrix for both filters. 



^k-l\k-l — Xk-l\k-l 



fc-1 fe-1 



fc-l|fe-l 



(43) 



(44) 



Thus, we consider the benefit of using the new constrained 
filter over the unconstrained Kalman Filter gained in one 
iteration. We can re-write all the constrained filter's equations 
in terms of the corresponding equations of the unconstrained 
Kalman Filter. 

Starting with Equation ( |32] |. we find that the state prediction 
remains the same over one iteration. 



~A Op 

— rk^k-iXk-i\k-i 



Xk\k-1 



(45a) 
(45b) 



Similarly, we find the error covariance prediction from 
Equation ( [33T l remains the same over one iteration. 



Pk\k-1 ^ FkM-lPk-l\k-lFLk^l + Qk,k-l 



i p 



k\k-l 



(46a) 
(46b) 



5 



The measurement prediction from Equation ([34]) is then 
modified as below. 



^k\k-l 







Zk\k-1 

Axk\k-i 



(47a) 



(47b) 



For the measurement residual from Equation dSSl l. we arrive 
at the following. 



ED 



Zk - HkXk\k-i 
b- Axk\k-i 



i^k 

b - Ax 



k\k-l 



(48a) 



(48b) 



The measurement residual covariance from Equation ( l36b 
can then be expressed as below. 



.A El 



tm 



(49a) 
(49b) 



HkPk\k-iH'i^ + Rk HkPk\k-iA' 
APk\k-iK APk\k-iA' 

Sk HkPk\k-iA'^ 
YAPk\k-iHl APk\k-iA' 

We are interested in finding ("S*^) ^ in a block structure. 
We notice that is a saddle point matrix of the form given in 
Appendix lAl The inverse of a saddle point matrix is given in 
a block matrix form in the appendix. We can apply this to 
Equation (|49l)F^ 

{Si)-;' ™ + [Su)-' H,P,^,_,A' 

(APfc|fc_iA' - APk\k-iK {Sk)-' HkPk\k-iA' 

(50a) 

APk^k-iHliSk)-' (50b) 
^ iSkV' + (Sk)-' HkPkik-iA' {APk\kA'y' 



APk\k~iH'k i^k) 



(50c) 
(50d) 

K'^A' {APk\kA'y' AKk (50e) 

In a similar manner using Equations (fTTI ). ( II 121 ). and dl 13t . 
we arrive at the following remaining terms in the block 
structure. 



= {>=>k) 



(si) 



K',A' {APk\kA') ' (51) 
- {APkikAy' AKk (52) 



"We know that Ag as defined in Appendix |A] will be nonsingular since it 
represents the measurement residual covariance Sk . If this matrix was singular, 
this would mean there exists no uncertainty in our measurement prediction 
or in our measurement, and thus there would be no ability to filter Similarly, 
we know that Jg as defined in Appendix |A] must also be nonsingular, which 
is equal to AP^^k-i^' (see Equation (49)). This term projects the predicted 
error covariance down to the constrained space. For well defined constraints 
(see Footnote |9), this will never be singular - it will have the same rank as 
A. 



{Si)/ ^{APkikA') ' (53) 

Applying this to Equations (|40at . we can find the first part 
of the Kalman Gain. 

(Ki)^ O P,|,_ii7[ {Si)/ + P,|,.iA' {Si)/ 

(54a) 

mm p „> (o ^-l 

+ Pkik-iKK'f,A' {APkikA')-' AKk 

(54b) 

- Pkik-iA' {APk\kA')-' AKk (54c) 
^ Kk - {Pk\k-i - Pk\k-iH'kK) (54d) 

A' {APk\kA')-' AKk (54e) 

™ Kk - Pk\kA' {APk\kA')-' AKk (54f) 

Following similar steps using Equations ( |44] |. ( fSTT l. (l53T l, and 
dl 14b . we can arrive at the other part of the Kalman Gain. 



{Ki)^^Pk\k^ {APk\kJ^) 



(55) 



We can then substitute our expressions for Ki directly into 
Equation dTTT i to find a simplified form of the updated state 
estimate. 



^k\k 



dUi} 



Xk\k-i+Kivi 



(56a) 



Xk\k-i + {Ki)^ vk + {Ki)^^ {b ~ Axk\k-i) 

(56b) 

Xk\k-i + KkVk - Pk\kA' {APk\kA')-' AKkVk 
+ Pk\kA' {APk\k^)-' {b - Axk\k-i) (56c) 
Xk\k - Pk\kA' {APk\kA') ' A {xk\k - Xk\k~i) 
+ Pk\kA' {APk\kA')-' {b ~ Axk\k-i) (56d) 
Xk\k - Pk\kA' {APk\kA')-' {Axk\k - b) 

(56e) 

Similarly, we can expand the updated error covariance in 
Equation ( |42] |. 



k\k 



(l-KiHi)P, 



(54), (55) 



k ^k ) ^k\k~l 

A\ IT ( T^A 



(57a) 



I - {Ki)^ Hk - {Ki)^Dk) Pkik-i (57b) 
l-KkHk + Pk\kA' {APk\kA')-' AKkHk 
-Pk\kA' {APk\k^)'' a) Pk\k-i (57c) 
{l-KkHk)Pk\k-i (57d) 

- Pk\kA' {APk\kA')-' A (I ^KkHk) Pk\k-i 

(57e) 

Pk\k - Pk\kA' {APk\kA')-' APkik (57f) 

Equations ( |56] l and f5T\ give us the improvement gained 
over an unconstrained Kalman Filter in a single iteration of 



m 
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the augmentation approach to constrained Kalman Fihering. 
We see that the covariance matrix can only get smaller since 
we are subtracting a positive semi-definite matrix from P^^. 
above PI 

B. Projecting the Unconstrained Estimate 

The second approach to equality constrained Kalman Filter- 
ing is to run an unconstrained Kalman Filter and to project 
the estimate down to the constrained space at each iteration. 
We can then feed the new constrained estimate into the 
unconstrained Kalman Filter and continue this process. Such a 
method can be described by the following minimization prob- 
lem for a given time-step k, in which is the constrained 
estimate, is the unconstrained estimate from the Kalman 
Filter equations, and is any positive definite symmetric 
weighting matrix^ 



^fc|fc = argmm|(a; - Xu\k)' Wk {x - Xk\k) : Ax ^ 



The best constrained estimate is then given below. 



^fc = - W-'A' {AW-^A') 



(58) 



(59) 



If we choose Wk — P^j^, we obtain the same solution as 
Equation ( |56] |. This is not obvious considering the differing 
approaches. The updated error covariance under this assump- 
tion will be the same as Equation dsTI i since x^^, = ^k\k- 
This choice of Wk is the most natural since it best describes 
the uncertainty in the state. One can also show that this choice 
leads to the smallest updated error covariance matrix (see 
e.g., [18])0 

In the more general case, we can still find the updated error 
covariance as a function of the unconstrained Kalman Filter's 
updated error covariance matrix as before. First, let us define 
the matrix T below|3 

T ^W-^A' {AW~^A'y^ (60) 
Equation (|59T l can then be re-written as follows. 

K\k = ik\k - T {Axk\k - h) (61) 
We can find a reduced form for Xk ~ S:k\k '^^ 

below0 



- x^k ^ Xk - Xk\k + T {Axk\k - b- {Axk - b)) (62a) 



Xk - Xk\k + T {Axk\k - Axk) 
- (I-TA) {xk\k - Xk) 



(62b) 
(62c) 



'''if A and B are covariance matrices, we say B is smaller than A if A~B 
is positive semidefinite. 

'^The superscript P notation is used to denote the "projected" constrained 
filter. 

'*That is, this choice of Wk makes P^^j, smaller than any other choice of 
Wk (see Footnote [14). 

'^Note that TA is a projection matrix, as is (I — TA), by definition. If A 
is poorly conditioned, we can use a QR factorization to avoid squaring the 
condition number 

'^Remember Axu — 6 = 0. 



Using the definition of the error covariance matrix, we arrive 
at the following expression. 



PL = E 



E 



^fe ^k\kj X^\k 



(63a) 



(I -TA) {xk\k - Xk) {xk\k - Xk)' (I -T^)' 

(63b) 

= {l-TA)Pkik(l-rA)' (63c) 
= Pk\k- TAPk\k - PkikA'T' + TAPk\kA'T' (63d) 
^Pk\k- TAPfeifc (63e) 

In the projection framework, two different filters can be 
constructed - one with a feedback loop, and one without. 
That is, the Kalman Filter can be run in real-time, and 
as a post-processing step, the unconstrained estimate and 
updated error covariance matrix can be reformulated in the 
constrained space; or alternatively, the constrained estimate 
and its associated updated error covariance matrix can be 
fed back into the system in real-time. A large benefit of 
incorporating constraints can be realized in both techniques, 
though the feedback system should generally outperform the 
system without feedback. 

C. Restricting the optimal Kalman Gain 

The third approach to equality constrained Kalman Filtering 
is to expand the updated state estimate term in Equation ( |25] | 
using Equation (fT2l) . 



A {xk\k-i + KkVk) 



(64) 



Then we can choose a Kalman Gain , that restricts the 
updated state estimate to be in the constrained space0 In 
the unconstrained case, we chose the optimal Kalman Gain 
Kk, by solving the minimization problem below which yields 
Equation (fT4l i. 



Kk = argmin trace [(I -KHk) Pk\k-i (I -KHk)' + KRkK'] 

(65) 

Now we seek the optimal that satisfies the constrained 
optimization problem written below for a given time-step k. 



= argmin trace [(I -KHk) Pk\k-i (I -KHk)' + KRkK'] 



s.t. A {xk\k-i + Kvk) = b 



(66) 



We will solve this problem using the method of Lagrange 
Multipliers. First, we take the steps below, using the vec 
notation (column stacking matrices so they appear as long 
vectors, see Appendix O to convert all appearances of K in 

"The superscript R notation is used to denote the "restricted kalman gain" 
constrained filter 
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Equation (|66] ) into long vectors. Let us begin by expanding 
the following term. 

trace [(I ^KHk) Pk\k-i (I -KHk)' + KRuK'] 
= trace [Pk\k-i - KHkPk\k-i - Pk\k~iH'^K' 
+KHkPk\k~iH'kK' + KRkK'] 

^ trace [P^ik-i - KHkPk\k-i - Pk\k-iH'kK' + KSkK'] 
= trace [Pfe|fe_i] - trace [KHkPk\k-i] 

- ti-ace [Pk\k-iH'kK'] + trace [KSkK'] 

(67a) 



(5fc I) = -vec [Pk\k-iH'k] 
n'{Sk<»I) = -vec [Pkik-iH'k]' 
This leads to the following value for fi. 

^fp- (5-1® I) vec [Pk\k-iK] 



(75) 



fTgl 
(14) 



-vec [P,|,_ii7^5-i] 



(76) 



We now expand the last three terms in Equation ( I67al i one 

,|20l 



at a timer 



trace [KHkPk\k-i] ^ vec 



[HkPk\k-l 



vec [iiT] 



(68) 



= vec[Pfe|fe_iHy'vec[if] 
trace [Pk\k-iH'kK'] ^ vec [if]' vec [Pk\k-iH'k\ (69) 



trace [X5feA"] *^ vec [K\ vec [if 5*^ 



vec [K]' {S®1) vec [isT] 



(70) 



Remembering that trace [Pfe|fe_i] is constant, our objective 
function can be written as below. 



(71) 



vec [K]' (I ®Sk) vec [K'] - vec [Pk\k^iH[] ' vec [K] 
-vec [if]' vec [Pk\k-iH'k] 

Using Equation ( I122l i on the equality constraints, our mini 
mization problem is the following. 



argmin vec [K]' {Sk I) vec [K] 



-vec [Ffc|fc_iii^]'vec [K] 
-vec [if]' vec [Pk\k-iHl] 
s.t. {v'f. >g) A) vec [if ] = - Axk\k-i 



(72) 



Further, we simplify this problem so the minimization 
problem has only one quadratic term. We complete the square 
as follows. We want to find the unknown variable fi which 
will cancel the linear term. Let the quadratic term appear as 
follows. Note that the non-"vec [if ] " term is dropped as it is 
irrelevant for the minimization problem. 

(vec [K] + /i)' {Sk ® I) (vec [K] + fi) (73) 
The Unear term in the expansion above is the following. 

vec [K]' {Sk ® I) M + Ai' {Sk ® I) vec [K] (74) 
So we require that the two equations below hold. 

-"We use the symmetry of Pj.|(j_i in Equation (68) and the symmetry of 
Sk in Equation (70). 



-vec [if fe] 

Using Equation ( 11201 ), our quadratic term in the minimiza- 
tion problem becomes the following. 

(vec [if - Kk])' {Sk ® I) (vec [K - Kk]) (77) 

Let i = vec [if — iffc]. Then our minimization problem 
becomes the following. 

iff = argmin £'{Sk'E)l)e 

S.t. (z/^ (E>A){e + vec [Kk]) = b- Axk\k-i 

(78) 

We can then re- write the constraint taking the vec [Kk] term 
to the other side as below. 

(i/^®A)£ Axk\k-i - {i^'k® A)vec[Kk] 

fT22l , . . r ^ 1 

5-Axfe|fe_i - vec[Aiffej/fe] ^^^^ 
= b - Axi,\k-i - AKkVk 
^ b-Axk\k 
This results in the following simplified form. 

iff = argmin i'{Sk(Eil)£ 

(80) 

s.t. {i^'i^ (g) A)l ~ b — Axk\k 

We form the Lagrangian C, for which we introduce q 
Lagrange Multipliers in vector A = (Ai, A2, . . . , A,)' 

C =e' {Sk ® I) <' - A' [{ly^ (E)A)i-b + Axkik] (81) 
We take the partial derivative with respect to ^F*! 



A' (4® A) 



(82) 



Similarly we can take the partial derivative with respect to 
the vector A. 



dC 



{i'l®A)l-b + Ax 



k\k 



(83) 



When both of these derivatives are set equal to the appropri- 
ate size zero vector, we have the solution to the system. Taking 
the transpose of Equation (|82] |. we can write this system as 
Mn = p with the following block definitions for M, n, and p. 



M 



'2Sk <E)l Vk® A'' 



(84) 



'We used the symmetry of {S^ <S> I) here. 
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n = 



P ■ 



(85) 
(86) 



0[mnx 1] 

b - Axk\k_ 

We solve this system for vector n in Appendix [D] The 
solution for i is copied below. 

(87) 

Bearing in mind that h — Axk\k — vec \h — Axk\}?\ , we can 
use Equation ( 1122b to re-write I as belowo 



the third part is the equality constraint, effectively still 
represents the measurement, with the prediction treated as a 
"pseudo-measurement" with its associated covariance. 



Xk\k-1 
Zk 

b 



(92) 



The matrix takes our state into the measurement space, 



as before. 



Ht = 



I 

A 



(93) 



vec 



A'{AA') Ub-Ax, 



k\k, 



Vk) 



^'kSk 



(88) 



The resulting matrix inside the vec operation is then an n 
by TO matrix. Remembering the definition for I, we notice 
that K — Kk also results in an rt by to matrix. Since both of 
the components inside the vec operation result in matrices of 
the same size, we can safely remove the vec operation from 
both sides. This results in the following optimal constrained 
Kalman Gain K?. 



{Axk\k - b) {v'^S^ Vfc) 



(89) 



Kk - A [AA ) ^^^k\k — yi^k^k '^k) i^k^k 

If we now substitute this Kalman Gain into Equation (fTSI) 
to find the constrained updated state estimate, we end up with 
the following. 



Xk\k-A'iAA') '{Axk\k-b) (90) 

This is of course equivalent to the result of Equation ( |59] ) 
with the weighting matrix Wk chosen as the identity matrix. 
The error covariance for this estimate is given by Equation 

■231 



D. Fusion Approach 

The fourth approach to equality constrained Kalman Filter- 
ing is to augment the constraints onto the system using the 
fusion interpretation to the Kalman Filter. In this case, we 
would like to fuse our state prediction with our measurement in 
the constrained space. Our system is then defined as below|3 



Now we define 



as the noise term, in which v9 is 



normally distributed with mean and covariance given by 
matrix R^. 



•^k\k — 
Vk 





(94) 



The block diagonal elements of covariance matrix i?^ 
represent the covariance of each element of w^. We define 
the covariance of the state estimate error at time-step k as 
Pk\k- Notice that contains no block off-diagonal elements 
implying no cross-correlations. However, in this formulation, 
cross-correlations can be modelled. 



Pk\k-1 








Rk 




(95) 



This method of expressing our problem can be thought of 
as a fusion of the state prediction and the new measurement 
at each iteration. The solution and covariance for the problem 
given in Equation [91] is printed below. 



^k\k 



{Hk) {Rk, 



Zk 



Pk\k = I iH. 



(Rh 



Hi 



(96) 



(97) 



Zk = h'j:{xk) + v^, 



' N (0, i?; 



(91) 



Here z^. 



and v'^ 



are all vectors, each having three 
distinct parts. The first part represents the prediction for the 
current time-step, the second part is the measurement, and 

-^Here we used the symmetry of 5^7^ and {y'l^S^^ i^k^ 

-^We can use the unconstrained or constrained Kalman Gain to find this 
en'or covariance matrix. Since the constrained Kalman Gain is suboptimal for 
the unconstrained problem, before projecting onto the constrained space, the 
constrained covariance will be different from the unconstrained covariance. 
However, the difference lies exactly in the space orthogonal to which the 
covariance is projected onto by Equation (63). The proof is omitted for brevity. 

-"'The supersciipt C notation is used to denote the "augmented-fusion" 
constrained filter 



However, the matrix i?^ is positive semi-definite now, and 
therefore singular, so the inverse is not well defined. Let us 
look at the inverse of the following saddle point matrix. The 
bottom left block will correspond exactly to the right-hand side 
of Equation ( |96] l, and the bottom right block will correspond 
to the negated right-hand side of Equation Wt\ - This can be 
verified by making the proper substitutions using Appendix lAl 



H^k 




(98) 



The statement for the inverse of the saddle point matrix 
made in Appendix lAl also holds in the case where all inverses 
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are replaced by the Moore-Penrose pseudo-inverse [53]. Tak- 
ing the pseudo-inverse, we can correctly express Equations 
^ and dlD belowH 



■^k\k 



k\k 



[0 I] 



-l + 


I" 








[0 I] 



R^k 







(99) 



(100) 



We have already shown that the Fusion interpretation of 
the filter is identical to the Kalman Filter in Section Ill-AI 
this method is mathematically equivalent to the method in 
Section Illl-AI in which we have also augmented pseudo- 
measurements. 



IV. Nonlinear Equality Constraints 

Since the equality constraints that we model are often 
nonlinear, it is important to make an extension to nonlinear 
equality constrained Kalman Filtering for the four methods 
discussed thus far. We replace the linear equality constraint 
on the state space by the following nonlinear constraint 
flfc {xk) = h, in which C/t (•) is a vector-valued function. 
The method based on augmenting the constraints presented 
in Sections IIII-AI and Illl-DI is trivially extended by using an 
Extended Kalman Filter 

Incorporating nonlinear equality constraints into the meth- 
ods described in Section Illl-BI and Section Illl-CI requires a 
more explicit change. If we linearize our constraint, [xk) — 
b, about the current state prediction x^ik-i^ we have the 
following. 



fcifc-i 



(101) 



V. Discussion of Methods 

Thus far, we have discussed four different methods for 
incorporating equality constraints into a Kalman Filter, and we 
have shown that three of these are mathematically equivalent to 
the projection method under the assumption that the weighting 
matrix Wk is chosen appropriately. As such, the projection 
method is a more general formulation. On the other hand, 
the augmentation methods provide a trivial extension to soft 
equality constrained Kalman Filtering by increasing the noise 
for the constraints, which are normally zero. In implemen- 
tations, there are some subtle differences. For instance, the 
augmentation methods requires a minimal adjustment to codes 
for an existing Kalman Filter or an Extended Kalman Filter 
- that is, we can pass in the augmented matrices and get the 
constrained estimate. This is especially advantageous for codes 
that use variations of the standard linear Kalman Filter (e.g., 
an Unscented Kalman Filter). 

There is another more transparent difference between these 
methods. In implementations, we are bound to receive numer- 
ical round-off error. While these methods can be mathemati- 
cally equivalent, we will not see the exact same result. The 
round-off error that causes the most trouble occurs when the 
updated error covariance matrices P^j, or P^^j, lose symmetry 
or positive definiteness. A way around this is to use the Joseph 
Form of the updated error covariance, which we discuss in 
more detail below. The error covariance matrix calculation 
using the fusion method P^^, should maintain positive defi- 
niteness and symmetry quite well in implementations as is. 



A. Numerical Preservation of the Updated Error Covariance 

We would like to find a form of Equation dSTj i that preserves 
symmetry and positive definiteness better. Let us start with the 
Joseph Form of the updated error covariance matrix given in 
Equation ( fTsT l. 



Here A is defined as the Jacobian of a evaluated at 
similar to before. This indicates, then, that the nonlinear 
constraint we would like to model can be approximated by 
the following linear constraint. 



Axk ~h + Ax 



" {^^k\k-i) 



(102) 



Then our projected state is given as in Section IIII-BI 
with A defined as above, and h replaced by the right hand 
side of Equation ( |102t . This linearizations is mathematically 
equivalent to the linearization step taken by the Extended 
Kalman Filter when augmenting the constraints. 

We can again take an iterative method, such as the Iterated 
Extended Kalman Filter, which takes multiple iterations and 
linearization per time-step. For the fusion implementation, an 
iterative algorithm is given in [22], [23]. 



P 



k\k 



{l-K^H^)Pk\k-i{l 
First, let us define the projection Tk below. 

Tk = l-Pk\kA' {APk\kA'y'A 



(103) 



(104) 



Then we see that Equation dSTj i can be written using Tk 
follows 



pA 
^k\k 



TkPi 



k\k 



(105) 



From this, the following easily follows using Equations ( fTSl l 
and (I57ali. 



I 



Tk (l-KkHk) 



(106) 



^^The pseudo-inverse may not be required if the matrix is invertible, the 
conditions for which are given in [54]. Further, if the matrix is invertible, the 
pseudo-inverse will be the true inverse. 



Equation (1106b will help us in reducing the term to the left 
of the "H-" sign in Equation ( I103l l. Let us focus on the right- 
side, K^R^ (K^)', for the moment. 
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.1 [m.m 



'Rk 0' 














(107a) 

= (107b) 
In terms of Ffe, we find the following to also be true. 



TkKk 



(108) 



We are now ready to use Equation d 1031 1 to find a simplified 
form for the constrained updated error covariance. 



pA 

^k\k — 



(1 -K^H^) Pk\k-i (I -K^'H^) (109a) 



(109b) 



f™' T^{l-K,Hu)Pk\k-i{l-KM'T', 

(109c) 

+ TkKuRkK',S'^, (109d) 
Vk[{l-KkHk)Pk\k~i{l-KkHk)' (109e) 

+KkRkK] r'fe (109f) 
TfePfcifcr;, (109g) 

To summarize, we can use Equation ( fTSl l or ( fTlT l to find 
Phik, and we can use Equation ( fTOSl ) or ( fT09] l to find P^^^,. 
In practice, we should use Equations (T3[ and ( 11091 ), when 
applicable, in order to maintain numerical stabihty. 

VI. Conclusions 

We have presented four approaches for incorporating state 
space equality constraints into a Kalman Filter and shown that 
three of them are special cases of the "Projection" method, 
which is a generalization that allows different weighting matri- 
ces when projecting the estimate. However, either of the two 
augmentation methods may prove easier in implementations 
since we can use existing Kalman Filter codes with minimal 
modifications. With the augmentation methods, we can also 
make a natural extension to incorporate soft equality con- 
straints, in which we allow the constraint to be slightly blurred 
by adding a proportionate amount of noise to the bottom right 
block entry of i?^ (see Equation (131]) ). For experiments, please 
refer to [47]. 

Appendix 

A. Inverse of a Saddle Point Matrix 

Ms is a saddle point matrix if it has the block form below|3 



Ms 



'As B's ■ 

Bs ~Cs 



(110) 



In the case that As is nonsingular and the Schur comple- 
ment Js — — {Cs + BsAg^Bg^ is also nonsingular in the 

-*The subscript S notation is used to diflerentiate these matrices from any 
matrices defined earlier. 



above equation, it is known that the inverse of this saddle point 
matrix can be expressed in the analytic block representation 
below (see e.g., [55]). 



Ms = 



A 



s "^s^^'s-^s^^sAs^ 
-Jo'BsA^' 



(111) 



B. Some Identities 

The following are identities that will prove useful in some 
of the earlier derivations of Section IIII-AI The matrices in 
these identities are used as defined in Sections and IIII-Al 

First Identity: 

APk\k-iA' - APk\k-iK {SkV^ HkPk\k^iA' (112a) 
CD 



APk\k-iA' - AKkHkPk\k-iA' 
= A{l-KkHk)Pk\k-iA' 

^ APk^kA' 



(112b) 
(112c) 

(112d) 



Second Identity: In the first step below, we make use of the 
symmetry of Pk\k-i and (S'fe)"^ 

(Sk)-^ HkPk\k-iA' {APk\kA')-' APkik-iK iSk)-' 

(113a) 

= (^Pk\k-iHi{Sky')' A' (APkikA')-' APk^k-iHiiSk)-' 
^ K'^A' (APk^kA'y' AKk (113b) 
Third Identity: 

(114a) 
(114b) 
(114c) 

(114d) 



Pk\k-1 ~ Pk\k-lBkKf. 
= Pfc|fc_l(I-H^ifO 

= {I-KkHk)Pk\k^i 

m p 

— ^kk 



Again, we have made use of the symmetry of Pk\k-i 
between Equations ( II 14bl ) and ( II 14cl i. 

C. Kron and Vec 

In this appendix, we provide some definitions used earlier 
in the chapter Given matrix A G M™><" and B e W^'^, we 
can define the right Kronecker product as below|3 



{A(E)B) 



ai.iB 

Om.lP 



ai.nB 

^tnjiB 



(115) 



Given appropriately sized matrices A, B, C, and D such that 
all operations below are well-defined, we have the following 
equalities. 



{A(g>By = {A'(g,B') 



(116) 



-^The indices m, n, p, and q and all matrix definitions are independent of 
any used earlier Also, the subscript notation ai_„ denotes the element in the 
first row and n-th coluimi of A, and so forth. 
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^ ^ {A-^®B-^) (117) 

{A (E)B){C®D)^ (AC ® BD) (118) 

We can also define the vectorization of an [m x n] matrix 
A, which is a linear transformation on a matrix that stacks the 
columns iteratively to form a long vector of size [mn x 1], as 
below. 

" ais' 

, 1 

ai,2 

dm, 2 
0-l.n 



Using the vec operator, we can state the trivial definition 
below. 



vec [A] 



(119) 



vec [A + B]= vec [A] + vec [B] 



(120) 



Combining the vec operator with the Kronecker product, we 
have the following. 



vec [AB] = {B' I) vec [A] 



(121) 



vec [ABC] = (C ® A) vec [B] (122) 
We can express the trace of a product of matrices as below. 

trace [AB] = vec [B']' vec [A] 



trace [ABC] = vec [B]' (I ®C) vec [A] 
= vec [A]' {l(g)B)vec [C] 
= vec [A]'(C(g)l) vec [B] 

For more information, please see [56]. 



(123) 

(124a) 
(124b) 
(124c) 



D. Solution to the system Mn — p 

Here we solve the system Mn = p from Equations (l84b . 
([85]) . and (|86] |. re-stated below, for vector n. 



2Sk 
i^'k^A 



1 1 ^fc (g) A'' 







[9x9] 





'e' 




0[mnxl] 




A 







(125) 



M is a saddle point matrix with the following equations to 
fit the block structure of Equation ( II 10b ^ 



-^We use Equation {116\ with B'^ to arrive at the same term for Bs in 
Equation )125) . 



As = 2Sk<»l 
Bs = ® A 

Cs = 0[gxg] 

We can calculate the term Ag^B'g. 

As'B's^[2{Sk®l)]-'ii^',®A)' 



(nmn} i 



(5^-1 1) (i/fe ® A') 



fml 1 



And as a result we have the following for J5. 

^-\Hsj:'>^k)®{AA') 

Jg^ is then, as below. 



(126) 
(127) 
(128) 



(129a) 
(129b) 

(129c) 

(130a) 
(130b) 



(Ola) 
(131b) 



For the upper right block of M ^, we then have the 
following expression. 



^s'B'sJs' = [{Sk'^^k) ® A'] [{jy'kS^'i^k) ' ® (AAr 

(132a) 



(132b) 



Since the first block element of p is a vector of zeros, we 
can solve for n to arrive at the following solution for £. 

{[S^'iyk K5,7Vfc)"'] ® [a' {AA')-']') (fo- Aifeife) 

(133) 

The vector of Lagrange Multipliers A is given below. 

WkSk'^'ky' ® {AAT'] {b - Axk\k) (134) 
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